#include "plane.h"
#include "math.h"
#include "util/mmgr.h"		// Needs to come in early

using namespace math;
// Constructors
Plane::Plane()
{
	m_Normal = Vec3	(0,1, 0);
	m_D = 0.0f;
}

Plane::Plane(float x, float y, float z, float w)
{
	m_Normal = Vec3	(x,y,z);
	m_D = w;
}


Plane::~Plane()
{

}

bool Plane::RayPlane(const Vec3& _vFrom, const Vec3& _vDir, Vec3& _vIntersect) const
{
	float	fAngle = m_Normal ^ _vDir;
	if (fAngle == 0) // parallel ?
		return false;

	float	fDist	= - ( (m_Normal ^ _vFrom) + m_D);
	float	fT		= fDist / fAngle;
	if ((fT < 0)) return false;
	_vIntersect		=  _vFrom + (_vDir*fT);
	return true;
}

void Plane::PlaneFromPoint(const Vec3& _vPoint, const Vec3& _vNormal)
{
	m_Normal	= _vNormal;
    m_Normal.Normalize();
	m_D		= - (	    (_vPoint.x * m_Normal.x) + 
						(_vPoint.y * m_Normal.y) +
						(_vPoint.z * m_Normal.z) );
}

void Plane::PlaneFromPoints(const Vec3& _A, const Vec3& _B, const Vec3& _C)
{
	m_Normal	= (_B - _A).Normalize() * (_C - _A).Normalize();
    m_Normal.Normalize();
    m_D		    = - m_Normal ^ _A;
}

bool Plane::Front(const Vec3& _vPoint) const
{
	// AX + BY + CZ + D >= 0?
	float fVal =(m_Normal.x * _vPoint.x) + 
				(m_Normal.y * _vPoint.y) + 
				(m_Normal.z * _vPoint.z) + 
				m_D;
	bool bFront = (fVal >= 0.0f);
	return bFront;
}

Plane	Plane::operator=(const  Plane &A)
{
	m_Normal = A.m_Normal;
	m_D = A.m_D;
	return *this;
}

Vec3		Plane::GetRight() const
{
	Vec3 Vec = Vec3(0, 0, 1);
    float Dot = fabsf(m_Normal ^ Vec);
	if (Dot > 0.99f)	// co-linear ?
	{
		Vec = Vec3(0, 1, 0);
	}

    return (Vec * m_Normal).Normalize();
}


const Vec3&	Plane::GetUp() const
{
	return m_Normal;
}

Vec3		Plane::GetForward() const
{
	Vec3 Vec = Vec3(1, 0, 0);
    float Dot = fabsf(m_Normal ^ Vec);
	if (Dot > 0.99f)	// co-linear ?
	{
		Vec = Vec3(0, 1, 0);
	}

    return (Vec * m_Normal).Normalize();
}
